/*
 * myqueue.c -- implementation for queue ops
 */

#include <myqueue.h>
#include <mythread.h>
#include <malloc.h>
#include <stdio.h>

void mythread_q_init(mythread_queue_t *headp, void *item, int prio)
{
  mythread_queue_t new, head;

  new = (mythread_queue_t) malloc(sizeof(struct mythread_queue));
  if (new == NULL) {
    printf("malloc error\n");
    exit(1);
  }
  new->item = item;
  new->prio = prio;

  new->prev = new;
  new->next = new;

  *headp = new;
}

/*void mythread_q_init(mythread_queue_t *headp, void *item)
{
  mythread_t th = (mythread_t) item;
  int prio = 10;

  if (th)
    prio = th->attr.param.sched_priority;
  mythread_q_init_prio(headp, item, prio);
}*/

void mythread_enq(mythread_queue_t *headp, void *item, int prio)
{
  mythread_queue_t new, head, next, prev;

  if (*headp == NULL) {
    mythread_q_init(headp, item, prio);
    return;
  }

  head = *headp;

  new = (mythread_queue_t) malloc(sizeof(struct mythread_queue));
  if (new == NULL) {
    printf("malloc error\n");
    exit(1);
  }
  new->item = item;
  new->prio = prio;

#ifndef TRASH
  if (head->prio > new->prio) {
    prev = head->prev;
    next = head;
  } else {
#endif
    prev = head;
    next = prev->next;
#ifndef TRASH
  }
#endif
  while (next != head) {
    if (next->prio > new->prio)
      break;
    prev = prev->next;
    next = next->next;
  }

  new->prev = prev;
  new->next = next;
  prev->next = new;
  next->prev = new;
#ifndef TRASH
  if (head->prio > new->prio)
    *headp = new;
#endif
}

/*void mythread_enq(mythread_queue_t *headp, void *item)
{
  mythread_t th = (mythread_t) item;
  int prio = 10;

  if (th)
    prio = th->attr.param.sched_priority;
  mythread_enq_prio(headp, item, prio);
}*/

void mythread_deq(mythread_queue_t *headp, void *item)
{
  mythread_queue_t old, q, head;

  head = *headp;

#ifdef TRASH
  if (head == NULL)
    return;
#endif

  q = head;
  do {
    if (q->item == item) {
      old = q;
      old->next->prev = q->prev;
      old->prev->next = q->next;
      if (old == head) {
	if (old->next == old)
	  *headp = (mythread_queue_t) NULL;
	else
	  *headp = old->next;
      }
      free(old);
      return; /* found */
    }
    q = q->next;
  } while (q != head);

  q = head;
  /* not found */
}
